#include "motion_cmd.h"
#include "log/zuclog.h"
#include "motion/mot_priv.h"
#include "errcode.h"
#include "motion/semintf.h"
#include "dyna/jaka_dyn_intf.h"
#include "torque_control_queue.h"
//#include "tp/tpmanager.hh"
// #include "motion/collision_detection.h"
#include "motion/motion_debug.h"
#include "rtbus/rtbus.h"
#include "zuc/nml_intf/motion_types.h"
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/robot_mgr.h"
#include "axisgroup/axisgroup_set.h"
#include "rob_common/robot_payload.h"
#include "axisgroup/pfsm_servo.h"
#include "safety/io/safeio.h"
#include "task/motion_al.h"

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD %d", fmt, group_id_, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD %d", fmt, group_id_, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD %d", fmt, group_id_, ##__VA_ARGS__)

static double saved_rapid_rate = 1.0;

int CmdSetTorqueControlEnable::process()
{
    MOTCMD_INFO("ZUCMOT_SET_TORQUE_CONTROL_ENABLE: %d, %d\n", torque_control_enable_, torque_last_period_);
    if (torque_control_enable_ && zucmotStatus->torque_control_settings.torque_control_enable)
    {
        return -1;
    }
    if (torque_control_enable_)
    {
        // powered on 及 ENABLE_FLAG 为机器人本体真实上电上使能状态
        if (!zucmotStatus->powered_on || !GET_MOTION_ENABLE_FLAG())
        {
            reportError(ERROR_ENTER_TORQUE_CONTROL_WHILE_OFF, "Can't enter torque control mode, while power off or disabled");
            return -1;
        }
        if (!GET_MOTION_INPOS_FLAG() || zucmotStatus->motion_state != ZUCMOT_MOTION_FREE)
        {
            reportError(ERROR_ENTER_TORQUE_CONTROL_WHILE_MOVING, "Can't enter torque control mode, while robot is moving or not in free mode");
            return -1;
        }
    }

    if (torque_control_enable_ && !zucmotStatus->torque_control_settings.torque_control_enable)
    {
        for (int i = 0; i < zucmotConfig->numJoints; i++)
        {
            // set_robot_joint_friction_compensate(i, 0);  // 伺服额外的补偿需要关闭
        }
        MOTCMD_INFO("Enter Torque Control Mode!\n");
        // 进入拖拽模式
        switch_to_drag_mode();
        for (int n = 0; n < ZUCMOT_MAX_JOINTS; n++) { zucmotStatus->torque_control_settings.current_torque_val[n] = 0; }
        zucmotStatus->torque_control_settings.torque_control_enable = 1;
        zucmotStatus->torque_control_settings.torque_last_period = torque_last_period_;
        zucmotStatus->torque_control_settings.torque_last_count = -1;
        int sem_id = sem_get(TORQUE_CONTROL_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            zucmotTorqueControlStruct->enable = 1;
            TorqueControlQueReset(&zucmotTorqueControlStruct->torque_que);  // 进入时清空列表
            sem_v(sem_id);
        }
    }
    if (!torque_control_enable_ && zucmotStatus->torque_control_settings.torque_control_enable)
    {
        MOTCMD_INFO("Exit Torque Control Mode!\n");
        // 退出拖拽模式TorqueControlQueReset
        if (GET_MOTION_DRAG_FLAG())
        {
            zucmotDebug->dragging = 0;
        }
        zucmotStatus->torque_control_settings.torque_control_enable = 0;  // 直接退出
        zucmotStatus->torque_control_settings.torque_last_count = -1;
        int sem_id = sem_get(TORQUE_CONTROL_SEM_KEY);
        if (sem_p(sem_id) == 0)
        {
            zucmotTorqueControlStruct->enable = 0;
            TorqueControlQueReset(&zucmotTorqueControlStruct->torque_que);  // 进入时清空列表
            sem_v(sem_id);
        }
        zucmotStatus->torque_control_settings.command_to_close = 1;
    }
    return 0;
}

int CmdTrackingEnable::process()
{
    // MOTCMD_INFO(" #################################################################### ZUCMOT_TRACKING_ENABLE "
    //             "#################################################################### \n");
    // zucmotStatus->tracking = 1;
    // zucmotStatus->still_dec = 0;
    // zucmotStatus->pre_singular = 0;
    // zucmotStatus->maxdis_judge = 0;
    // zucmotStatus->conveyorMaxDistance = max_distance;
    // zucmotStatus->conveyorPulseEquivalent = conveyorPulseEquivalent;
    // zucmotStatus->last_vel_for_convy = 0.0;
    // zucmotStatus->first_cycle_in_tracking = 1;
    // zucmotStatus->status_acc_convyr = 0;
    // zucmotStatus->last_theta = 0.0;
    // zucmotStatus->convyr_type = convyr_type;

    // if (convyr_type_ == 1)
    // {
    //    //     tpManager_tpSetTrackLinearCfgEna(1, conveyorPulseEquivalent, conveyorDir);
    //     zucmotStatus->conveyorPulseEquivalent = conveyorPulseEquivalent;
    //     zucmotStatus->conveyorDir.x = conveyorDir.x;
    //     zucmotStatus->conveyorDir.y = conveyorDir.y;
    //     zucmotStatus->conveyorDir.z = conveyorDir.z;
    // }
    // else if (convyr_type_ == 2)
    // {
    //    //     tpManager_tpSetTrackCircularCfgEna(1, conveyorPulseEquivalent, o_to_world, rotate_tool);
    //     zucmotStatus->conveyorPulseEquivalent = conveyorPulseEquivalent;
    //     zucmotStatus->o_to_world = o_to_world;
    //     zucmotStatus->rotate_tool = rotate_tool;
    // }
    // if (zucmotStatus->conveyorMaxDistance < 1e-5)
    // {
    //     zucmotStatus->ValidDistance = 0;
    // }
    // else
    // {
    //     zucmotStatus->ValidDistance = 1;
    // }
    //    // if (tpManager_GetConveyAnyChanged() != 0 && tpManager_GetConveyEnabled())
    // {  // we have DIO's to turn on or off
    //     if (convyr_type_ == 1)
    //     {
    //    //         tpManager_GetConveyDir(&zucmotStatus->conveyorDir);
    //         pmCartUnitEq(&zucmotStatus->conveyorDir);
    //         if (!zucmotConfig->lctsynch)
    //         {
    //             zucmotStatus->status_acc_convyr = 3;
    //         }
    //     }
    //     else if (convyr_type_ == 2)
    //     {
    //    //         tpManager_GetConveyO2world(&zucmotStatus->o_to_world);
    //         zucmotStatus->r_for_convyr = 0.0;
    //    //         zucmotStatus->rotate_tool = tpManager_GetConveyRotTool();

    //         // MOTCMD_INFO("[tpToggleTrackCfg] ---> zucmotStatus->o_to_world : \n");
    //         // MOTCMD_INFO("zucmotStatus->o_to_world.rot = : \n");
    //         // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.x, zucmotStatus->o_to_world.rot.y.x, zucmotStatus->o_to_world.rot.z.x);
    //         // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.y, zucmotStatus->o_to_world.rot.y.y, zucmotStatus->o_to_world.rot.z.y);
    //         // MOTCMD_INFO("%f %f %f\n", zucmotStatus->o_to_world.rot.x.z, zucmotStatus->o_to_world.rot.y.z, zucmotStatus->o_to_world.rot.z.z);
    //     }
    //     if (fabs(zucmotStatus->conveyorPulseEquivalent) < 1e-8)
    //     {
    //         zucmotStatus->status_acc_convyr = 3;
    //     }
    //     if (zucmotConfig->link_di < 0 || zucmotConfig->link_di > 8 || !zucmotStatus->get_conveyor_ori)
    //     {
    //         long int convayorCnt = zucmotStatus->last_convayorCnt;
    //         MOTCMD_INFO("-------------  zucmotConfig->link_di = %d, [ZUCMOT_TRACKING_ENABLE]------------:convayorCnt = %ld, conveyorPulseEquivalent = %f\n ",
    //                     zucmotConfig->link_di,
    //                     convayorCnt,
    //                     zucmotStatus->conveyorPulseEquivalent);
    //         zucmotStatus->turns_count = 0;
    //         zucmotStatus->conveyorOrigin = convayorCnt;
    //         zucmotStatus->last_convayorCnt = convayorCnt;
    //         zucmotStatus->uhiData.pulses = zucmotStatus->conveyorOrigin;
    //         zucmotStatus->get_conveyor_ori = 1;
    //     }
    // }

    // if (zucmotConfig->link_di < 0)
    // {
    //     long int convayorCnt = zucmotStatus->last_convayorCnt;
    //     MOTCMD_INFO("-------------  zucmotConfig->link_di = %d, [ZUCMOT_TRACKING_ENABLE]------------:convayorCnt = %ld, conveyorPulseEquivalent = %f\n ",
    //                 zucmotConfig->link_di,
    //                 convayorCnt,
    //                 zucmotStatus->conveyorPulseEquivalent);
    //     zucmotStatus->turns_count = 0;
    //     zucmotStatus->conveyorOrigin = convayorCnt;
    //     zucmotStatus->last_convayorCnt = convayorCnt;
    //     zucmotStatus->uhiData.pulses = zucmotStatus->conveyorOrigin;
    // }
    // MOTCMD_INFO(" ---------------------------------------- [ZUCMOT_TRACKING_ENABLE] ---------------------------------------- "
    //             ":zucmotStatus->last_convayorCnt =%ld, zucmotStatus->uhiData.pulses = %ld\n",
    //             zucmotStatus->last_convayorCnt,
    //             zucmotStatus->uhiData.pulses);

    // if (get_robot_joint_collision_sensitivity() != 0x0F)
    // {
    //     set_robot_joint_collision_sensitivity(0x0F);
    // }
    return 0;
}

int CmdToppraFinishLeftTc::process()
{
    MOTCMD_INFO("  command ZUCMOT_TOPPRA_FINISH_LEFT_TC  \n");
    zucmotStatus->finish_read = 1;
    return 0;
}

int CmdToppraFirstCommand::process()
{
    MOTCMD_INFO("  command ZUCMOT_TOPPRA_FIRST_COMMAND  \n");
    zucmotStatus->finish_read = 0;
    return 0;
}

int CmdTrackingDisable::process()
{
    // MOTCMD_INFO(" ******************************************************** TRACKING DISABLE...\n");
    // // reportInfo(INFO_CONVEYOR_TRACK_DISABLED, "conveyor track function disabled");
    // temp_fflags = ctrl_fflags;
    // temp_iflags = ctrl_iflags;
    // /* set conveyor tracking cfg */
    // zucmotStatus->tracking = 0;
    //    // tpManager_tpDisableTrack();
    // zucmotStatus->conveyorPulseEquivalent = 0;
    // zucmotStatus->status_acc_convyr = 3;

    // /* get real joint position according to joints */
    // int jntNum = 0;
    // double jointPos[ZUCMOT_MAX_JOINTS] = {0}, tar_pos_joint[ZUCMOT_MAX_JOINTS] = {0};
    // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++)
    // {
    //     jointPos[jntNum] = joints[jntNum].pos_cmd;
    //     tar_pos_joint[jntNum] = joints[jntNum].pos_cmd;
    // }
    // /* get the real carte_pos by kinematic forward */
    // //KINEMATICS_FORWARD_FLAGS l_fflags = 0;
    // //KINEMATICS_INVERSE_FLAGS l_iflags = 0;
    // kinematicsForward(&zucmotConfig->tool_offset,
    //                   jointPos,
    //                   &zucmotStatus->disable_convyr_real_pos,
    //                   &ctrl_fflags,
    //                   &ctrl_iflags,
    //                   &zucmotConfig->base_offset,
    //                   &zucmotConfig->user_offset);
    // //$1(ctrl_fflags);
    // //zucmotSet_CtrIFLAGES(ctrl_iflags);
    // int result = 0;
    // ZucPose tar_pos;
    // tar_pos = zucmotStatus->disable_convyr_real_pos;  //init tar_pos
    // if ((zucmotStatus->convyr_type == 1 && zucmotConfig->lctsynch) || zucmotStatus->convyr_type == 2)
    // {
    //     if (zucmotStatus->convyr_type == 2)
    //     {  //若是圆形传送带跟踪，则求减速至零转过的角度并根据旋转变换求速度为零时的笛卡尔和关节位置
    //         if (zucmotStatus->r_for_convyr != 0.0)
    //         {
    //             zucmotStatus->left_theta =
    //                 pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotStatus->r_for_convyr / zucmotConfig->synch_acc;  //zucmotStatus->r_for_convyr==0???,
    //         }
    //         else
    //         {
    //             zucmotStatus->left_theta = 0.0;
    //         }
    //     }
    //     else
    //     {
    //         zucmotStatus->left_theta = pmSq(zucmotStatus->last_vel_for_convy) / 2.0 / zucmotConfig->synch_acc;  //v^2 = 2*a*s
    //     }

    //     /* get the pos when the vel(omega) of robot is zero */
    //     get_tar_pos(zucmotStatus->left_theta, &tar_pos);
    //     result = kinematicsInverse(
    //         &zucmotConfig->tool_offset, &tar_pos, tar_pos_joint, &ctrl_iflags, &ctrl_fflags, &zucmotConfig->base_offset, &zucmotConfig->user_offset);
    //     MOTCMD_INFO(" ######################## ZUCMOT_TRACKING_DISABLE left_theta = %f ########################\n", zucmotStatus->left_theta);
    //     MOTCMD_INFO("tar_pos = %f %f %f %f %f %f\n, tar_pos_joint = %f %f %f %f %f %f\n",
    //                 tar_pos.tran.x,
    //                 tar_pos.tran.y,
    //                 tar_pos.tran.z,
    //                 tar_pos.a,
    //                 tar_pos.b,
    //                 tar_pos.c,
    //                 tar_pos_joint[0],
    //                 tar_pos_joint[1],
    //                 tar_pos_joint[2],
    //                 tar_pos_joint[3],
    //                 tar_pos_joint[4],
    //                 tar_pos_joint[5]);
    //     if (result != 0 || (zucmotStatus->convyr_type == 2 && zucmotStatus->r_for_convyr < 1e-8))
    //     {
    //         // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
    //    //         tpManager_tpSetCurJpos(jointPos);
    //         // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
    //    //         tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
    //         // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;
    //    //         tpManager_tpGetPose(&zucmotStatus->carte_pos_desired);
    //         zucmotStatus->still_dec = 0;
    //     }
    //     else
    //     {
    //    //         tpManager_tpSetPos_JPos(&tar_pos, tar_pos_joint);
    //         zucmotStatus->still_dec = 1;
    //     }
    // }

    // else
    // {
    //     // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { zucmotDebug->coord_tp.currentPos_joint[jntNum] = jointPos[jntNum]; }
    //     // tpSetPos(&zucmotDebug->coord_tp, &zucmotStatus->disable_convyr_real_pos, jointPos);
    //     // zucmotStatus->carte_pos_desired = zucmotDebug->coord_tp.currentPos;

    //    //     tpManager_tpSetCurJpos(jointPos);
    //    //     tpManager_tpSetPos_JPos(&zucmotStatus->disable_convyr_real_pos, jointPos);
    //    //     tpManager_tpGetPose(&zucmotStatus->carte_pos_desired);
    //     zucmotStatus->still_dec = 0;
    // }
    // if (get_robot_joint_collision_sensitivity() == 0x0F)
    // {
    //     // in case of unexpected exit from admittance or drag
    //     set_robot_joint_collision_sensitivity(zucmotConfig->safetyCfg.clsn_sensitivity);
    // }
    return 0;
}

static void zucmot_abort()
{
    // int joint_num = 0;
    // int axis_num = 0;
    // zucmot_joint_t* joint = NULL;
    // zucmot_axis_t* axis = NULL;
    // // corrdinate 模式下不能直接退出力控！
    // if (zucmotStatus->compliantType == 1 && GET_MOTION_SERVOMOVEOP_FLAG())
    // {
    //     //MOTCMD_INFO(">>>aborting motion cmd2\n");
    //     zucmotStatus->compliantType = 0;
    // }
    // /* check for coord or free space motion active */
    // if (GET_MOTION_TELEOP_FLAG() || GET_MOTION_TELEOP_TOOL_FLAG())
    // {
    //     for (axis_num = 0; axis_num < ZUCMOT_MAX_AXIS; axis_num++)
    //     {
    //         /* point to joint struct */
    //         axis = &axes[axis_num];
    //         /* tell teleop planner to stop */
    //         axis->teleop_tp.enable = 0;
    //         axis->teleop_tool_tp.enable = 0;
    //     }
    // }
    // else if (GET_MOTION_COORD_FLAG())
    // {
    //     // MOTCMD_INFO(" ##################################################################################### zucmot_abort\n");
    //    //     if (tpManager_tpCollision_rebound() == 0)
    //     {
    //    //         tpManager_tpabort();
    //     }
    // }
    // else
    // {
    //     if (safety::SafeIOMgr::instance().is_protective_stop())
    //     {
    //         return;
    //     }
    //     for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    //     {
    //         /* point to joint struct */
    //         joints[joint_num].free_tp.enable = 0;
    //         /* stop homing if in progress */
    //         if (joints[joint_num].home_state != HOME_IDLE)
    //         {
    //             joints[joint_num].home_state = HOME_ABORT;
    //         }
    //     }
    // }
    // SET_MOTION_ERROR_FLAG(0);
    // /* clear joint errors (regardless of mode) */
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     /* point to joint struct */
    //     joint = &joints[joint_num];
    //     /* update status flags */
    //     SET_JOINT_ERROR_FLAG(joint, 0);
    //     SET_JOINT_FAULT_FLAG(joint, 0);
    // }
    // zucmotStatus->paused = 0;
    // zucmotStatus->abort_flag_for_GM = 1;
    // if (zucmotStatus->category_one_stop_program_pause == 1)
    // {
    //     SET_MOTION_COORD_FLAG(0);
    // }
}

int CmdAbortJointAndTraj::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->motion_abort();
    zucmot_abort();
    return 0;
}

int CmdAbort::process()
{
    /* abort motion , can happen at any time */
    /* this command attempts to stop all machine motion. it looks at
    		 the current mode and acts accordingly, if in teleop mode, it
    		 sets the desired velocities to zero, if in coordinated mode,
    		 it calls the traj planner abort function (don't know what that
    		 does yet), and if in free mode, it disables the free mode traj
    		 planners which stops joint motion */
    MOTCMD_DEBUG("ABORT");
    //MOTCMD_INFO(">>>aborting motion cmd\n");
    //turn off force control when aborting servo move mode to avoid pos jump
    zucmot_abort();
    if (zucmotDebug->stepping == 1)
        zucmotDebug->stepping = 0;
    return 0;
}

int CmdFree::process()
{
    // MOTCMD_DEBUG("ZUCMOT_FREE");
    // auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    // if (!robot.get())
    // {
    //     return -1;
    // }
    // robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_FREE);
    return 0;
}

int CmdCoord::process()
{
    /* change the mode to coordinated axis motion */
    /* can be done at any time */
    /* this code doesn't actually make the transition, it merely
    		 tests a condition and then sets a flag requesting the
    		 transition */
    /* set the zucmotDebug->coordinating flag to defer transition to
    		 controller cycle */
    // MOTCMD_DEBUG("COORD");
    // zucmotDebug->coordinating = 1;
    // zucmotDebug->teleoperating = 0;
    // zucmotDebug->teleoperating_tool = 0;
    // zucmotDebug->dragging = 0;
    // zucmotDebug->admitting = 0;
    // if (zucmotConfig->kinType != KINEMATICS_IDENTITY)
    // {
    //     if (!checkAllHomed())
    //     {
    //         // reportError(ROBOT_NOT_HOMED, ("all joints must be homed before going into coordinated mode"));
    //         zucmotDebug->coordinating = 0;
    //         // printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>%s[%d]   zucmotDebug->coordinating = 0 \n", __FILE__, __LINE__);
    //         return 0;
    //     }
    // }
    return 0;
}

int CmdTeleop::process()
{
    // MOTCMD_DEBUG("TELEOP");
    // switch_to_teleop_mode();
    return 0;
}

int CmdTeleopTool::process()
{
    // MOTCMD_DEBUG("TELEOP_TOOL");
    // switch_to_teleop_tool_mode();
    return 0;
}

int CmdAdmittance::process()
{
    // MOTCMD_DEBUG("ADMITTANCE");
    // switch_to_admittance_mode();
    return 0;
}

int CmdDrag::process()
{
    if (zucmotStatus->sFuncDISettings.drag_limit != 0 && zucmotStatus->three_position_enable_limit == 1)
    {
        //如果配置文件中配置了三位置使能限制拖拽，并且三位置使能为触发状态禁止进入拖拽模式
        reportError(ERROR_THREE_POSITION_ENABLING_LIMIT, "Can't drag when the three positions enable limits");
        return 0;
    }
    MOTCMD_DEBUG("DRAG");
    if (GET_MOTION_ENABLE_FLAG())
    {
        switch_to_drag_mode();
    }
    return 0;
}

int CmdSetEndForceCondition::process()
{
    //    tpManager_tpSetEndForceCond(axis_, endForceCond_);
    return 0;
}

int CmdSetRobot7DofInvFlag::process()
{
    MOTCMD_INFO("CmdSetRobot7DofInvFlag, %d", inv_flag_);
    zucmotConfig->robot_7dof_inform.inv_flag = inv_flag_;
    // set_dof_7_inv_type(inv_flag_);
    zucmot_config_change();
    return 0;
}

int CmdSetRobotFullDhFlag::process()
{
    MOTCMD_INFO("CmdSetRobotFullDhFlag, %d", robot_full_dh_flag_);
    // setFullDHCom(robot_full_dh_flag_);
    zucmot_config_change();
    return 0;
}

int CmdSetRobot7DofArmAngle::process()
{
    MOTCMD_INFO("CmdSetRobot7DofArmAngle, %lf", arm_angle_);
    zucmotConfig->robot_7dof_inform.arm_angle = arm_angle_;
    // updataRobot_Arm_Angle_fixed(arm_angle_);
    zucmot_config_change();
    return 0;
}

int CmdSetRobot7DofKineParamAlphaK::process()
{
    MOTCMD_INFO("alpha %f k %f \n", k_, alpha_);
    joint_lim_Algo_param setting;
    setting.k = k_;
    setting.alpha = alpha_;
    // updateRobot_joint_lim_Algo_param(&setting);
    zucmot_config_change();
    return 0;
}

int CmdDisable::active_callback()
{
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
    if (!status.is_act_inpos)
    {
        MOTCMD_ERR("robot is not inpos, can't disable");
        return -1;
    }
    return 0;
}

int CmdDisable::process()
{
    MOTCMD_DEBUG("ZUCMOT_DISABLE");
    axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->do_enable(false);
    return 0;
}

int CmdEnable::active_callback()
{
    // if (zucmotStatus.protective_stop == 1)
    // {
    //     rcs_print("Cannot enable traj when in protective_stop status\n");
    //     return -1;
    // }
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
    if (status.is_enable || status.is_enabling)
    {
        return -1;
    }
    if (!status.is_power_on)
    {
        zucOperatorError(ROBOT_POWERED_OFF, "robot is now powered-off, please turn on it");
        return -1;
    }

    // if (zucStatus->cab_type == 1)  //v1.7.1后不支持cabv1
    // {
    //     zucOperatorError(UNSUPPORTED_CABINET, "unsupport cabv1#{\"fixed_key\":[\"CabV1\"]}");
    //     return -1;
    // }
    // if (!zuc_sim && !zucStatus->motion.is_power_on)
    // {
    //     zucOperatorError(CAN_ENA_ROBOT_WHRN_READ_PARAMS, "cannot enable robot when reading D-H or dynamics parameters from servo side");
    //     return -1;
    // }
    // if (!zucStatus->motion.is_enabled)
    // {
    //     if (zucStatus->motion.robot_model_data.model == MODEL_UNSUPPORTED)
    //     {
    //         zucOperatorError(UNSUPPORTED_ROBOT_MODEL, "model of connected robot is now unsupported");
    //         return -1;
    //     }

    //     if (true == zucStatus->io.func_io_req.servo_disable_req)  // funcio servo_disable_req is true
    //     {
    //         zucOperatorError(ERR_FDI_SERVO_ON_LOCKED, "robot enable function is locked by servo-off-request of funcIO.");
    //         return -1;
    //     }

    //     task_set_state->zucTrajEnable();
    //     zucMotorsServoOn(true);

    //     // if( EMErgencyStop)
    //     // zucTrajSetMode(ZUC_TRAJ_MODE_FREE);
    //     //  zucTrajSetMode(ZUC_TRAJ_MODE_FREE);
    //     for (t = 0; t < zucStatus->motion.traj.joints; t++) { zucJointEnable(t); }
    //     zucJointHome(-1);
    // }
    // else
    // {
    //     zucOperatorError(INFO_ROBOT_HAS_BEEN_ENABLED, "The robot has been enabled.");
    //     return -1;
    // }
    return 0;
}

int CmdEnable::process()
{
    MOTCMD_DEBUG("ENABLE");
    axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->do_enable(true);
    return 0;
}

int CmdSetMaintenanceMode::process()
{
    MOTCMD_INFO("CmdSetMaintenanceMode, %d\n", maintenance_);
    int sem_id = sem_get(EXTIO_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        zucmotStatus->maintenance_status = maintenance_;
    }
    return 0;
}

int CmdSetPayloadTorqsensor::process()
{
    MOTCMD_INFO("ZUCMOT_SETPAYLOAD_TORQSENSOR: %f, p: %f, %f, %f\n", payload_.m, payload_.p[0], payload_.p[1], payload_.p[2]);
    int sem_id = sem_get(SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        // zucmotSensorStruct->sensorConfig.payload = payload_;
        sem_v(sem_id);
    }
    return 0;
}

int CmdSetOperationmode::process()
{
    MOTCMD_INFO("set operation mode %d\n", operationMode_);
    zucmotStatus->operation_mode = operationMode_;
    return 0;
}

int CmdSetMaxFeedOverride::process()
{
    MOTCMD_INFO("CmdSetMaxFeedOverride, %lf", maxFeedScale_);
    zucmotConfig->maxFeedScale = maxFeedScale_;
    zucmot_config_change();
    return 0;
}

int CmdSetupArcBlends::process()
{
    MOTCMD_INFO("CmdSetupArcBlends, %d, %d, %d, %d, %lf, %lf",
                setuparcblends_.arcBlendEnable,
                setuparcblends_.arcBlendFallbackEnable,
                setuparcblends_.arcBlendOptDepth,
                setuparcblends_.arcBlendGapCycles,
                setuparcblends_.arcBlendRampFreq,
                setuparcblends_.arcBlendTangentKinkRatio);

    zucmotConfig->arcBlendEnable = setuparcblends_.arcBlendEnable;
    zucmotConfig->arcBlendFallbackEnable = setuparcblends_.arcBlendFallbackEnable;
    zucmotConfig->arcBlendOptDepth = setuparcblends_.arcBlendOptDepth;
    zucmotConfig->arcBlendGapCycles = setuparcblends_.arcBlendGapCycles;
    zucmotConfig->arcBlendRampFreq = setuparcblends_.arcBlendRampFreq;
    zucmotConfig->arcBlendTangentKinkRatio = setuparcblends_.arcBlendTangentKinkRatio;

    //    tpManager_SetArcBlendsParam(&setuparcblends_);
    zucmot_config_change();
    return 0;
}

int CmdExitProtect::process()
{
    MOTCMD_INFO("CmdExitProtect");
    zucmot_hal_data->error_triggered = 0;
    zucmot_hal_data->motion_error_code = 0;
    MOTCMD_INFO("error reset \n");
    // setExitProtectiveStop(1);
    if (type_ == 0 && safety::SafeIOMgr::instance().is_protective_stop())
    {
        reportInfo(INFO_ROBOT_COLLISION_STOP_RESET, "robot exit from collision protective stop");
        zucmotStatus->protective_stop = 0;
        zucmotDebug->teleoperating = 0;
        // if (zucmotConfig->safetyCfg.clsnOpt.reactionType != 1)
        // {
        //     zucmotDebug->coordinating = 0;
        // }
        zucmotDebug->teleoperating_tool = 0;
        zucmotDebug->dragging = 0;
        zucmotDebug->admitting = 0;
    }
    else if (type_ == 1 && zucmotStatus->safetyguard_lock == 1)
    {
        if (zucmot_hal_data->stop_mode_req == 1 || zucmot_hal_data->safetyguard_req == 1)
        {
            reportError(FAILED_RESET_PROTECTIVE_STOP, "cannot reset protective stop mode when DI still triggered\n");
        }
        else
        {
            zucmotStatus->safetyguard_lock = 0;
        }
    }
    else if (type_ == 2 && zucmotStatus->is_safezone_limited == 1)
    {
        //安全平面触发确认
        zucmotStatus->safezone_confirm = 1;
        zucmotStatus->safetyguard_lock = 0;
        MOTCMD_INFO("SafeZone confirmation \n");
    }
    return 0;
}

int CmdPercentageMode::process()
{
    MOTCMD_INFO("CmdPercentageMode, %d", level_);
    if (level_ == 0)
    {
        if (zucmotStatus->percentage_mode_level != 0 && !zucmotStatus->safetyguard_lock)
        {
            if (zucmotStatus->percentage_mode_level == 3)
            {
                reportInfo(INFO_ROBOT_PROTECTIVE_STOP_RESET, "robot exit protective stop");
            }
            else
            {
                reportInfo(INFO_ROBOT_PERCENTAGE_MODE_RESET, "robot exit from percentage mode");
            }
            zucmotStatus->rapid_scale = saved_rapid_rate;
            zucmotStatus->percentage_mode_level = level_;
        }
    }
    else if (level_ == 1 && zucmotStatus->percentage_mode_level != 1)
    {
        if (zucmotStatus->percentage_mode_level == 0)
        {
            saved_rapid_rate = zucmotStatus->rapid_scale;
        }
        if (!zucmotStatus->safetyguard_lock)
        {
            reportInfo(INFO_ROBOT_PERCENTAGE_MODE_LEVEL1, "robot in percentage mode (leve 1)");
            zucmotStatus->rapid_scale = zucmotStatus->percentage_mode_level1_val;
            zucmotStatus->percentage_mode_level = level_;
        }
    }
    else if (level_ == 2 && zucmotStatus->percentage_mode_level != 2)
    {
        if (zucmotStatus->percentage_mode_level == 0)
        {
            saved_rapid_rate = zucmotStatus->rapid_scale;
        }
        if (!zucmotStatus->safetyguard_lock)
        {
            reportInfo(INFO_ROBOT_PERCENTAGE_MODE_LEVEL2, "robot in percentage mode (leve 2)");
            zucmotStatus->rapid_scale = zucmotStatus->percentage_mode_level2_val;
            zucmotStatus->percentage_mode_level = level_;
        }
    }
    else if (level_ == 3 && zucmotStatus->percentage_mode_level != 3)
    {
        if (zucmotStatus->percentage_mode_level == 0)
        {
            saved_rapid_rate = zucmotStatus->rapid_scale;
        }
        zucmotStatus->rapid_scale = 0.0;
        zucmotStatus->percentage_mode_level = level_;
        zucmotStatus->safetyguard_lock = 1;
        MOTCMD_INFO("robot in protective stop \n");
        reportInfo(INFO_ROBOT_PROTECTIVE_STOP, "robot in protective stop");
    }
    return 0;
}

int CmdReduceMode::process()
{
    if (mode_ == 0)
    {
        //退出缩减模式
        zucmotStatus->reduce_mode = 0;
        // enableSatfyReduce(0);
        MOTCMD_INFO("ZUCMOT_REDUCE_MODE = %d \n", mode_);
        reportInfo(INFO_ROBOT_REDUCE_MODE_RESET, "robot exit from reduce mode");
    }
    else if (mode_ == 1)
    {
        //进入缩减模式
        zucmotStatus->reduce_mode = 1;
        // enableSatfyReduce(1);
        MOTCMD_INFO("ZUCMOT_REDUCE_MODE = %d \n", mode_);
        reportInfo(INFO_ROBOT_REDUCE_MODE, "robot in reduced mode");
    }
    return 0;
}

int CmdSetPercentageModeLevelVal::process()
{
    MOTCMD_INFO("ZUCMOT_SET_PERCENTAGE_MODE_LEVEL_VAL = %lf, %lf \n", level1_val_, level2_val_);
    zucmotStatus->percentage_mode_level1_val = level1_val_;
    zucmotStatus->percentage_mode_level2_val = level2_val_;
    return 0;
}

int CmdSetClsnAccLimit::process()
{
    /* set the max axis acc */
    /* can be done at any time */
    MOTCMD_INFO("ZUCMOT_SET_CLSN_ACC_LIMIT: %f %f\n", pos_clsn_acc_, ori_clsn_acc_);
    zucmot_config_change();

    ClsnStopAcc clsn_stop_acc;
    clsn_stop_acc.pos_clsn_acc = pos_clsn_acc_;
    clsn_stop_acc.ori_clsn_acc = ori_clsn_acc_;
    zucmotConfig->safetyCfg.clsn_stop_acc = clsn_stop_acc;
    //    tpManager_tpSetClsnStopAcc(clsn_stop_acc);
    return 0;
}

int CmdSetTrajsmooth::process()
{
    MOTCMD_INFO("CmdSetTrajsmooth %d, %d", trqlimitActive_, firfltActive_);
    //    tpManager_tpTrajSmooth(trqlimitActive_, firfltActive_);
    return 0;
}

int CmdSetTrackingConfig::process()
{
    MOTCMD_INFO("CmdSetTrackingConfig %d, %d, %lf, %d", link_di_, com_cycle_num_, synch_acc_, lctsynch_);
    zucmotConfig->link_di = link_di_;
    zucmotConfig->com_cycle_num = com_cycle_num_;
    zucmotConfig->synch_acc = synch_acc_;
    zucmotConfig->lctsynch = lctsynch_;
    zucmot_config_change();
    return 0;
}

int CmdSetFullDhCom::process()
{
    MOTCMD_INFO("CmdSetFullDhCom %d", full_dh_com_);
    zucmotConfig->full_dh_com = full_dh_com_;
    // setFullDHCom(full_dh_com_);
    zucmot_config_change();
    return 0;
}

int CmdSetEnableLimitLinearVelocity::process()
{
    MOTCMD_INFO("CmdSetEnableLimitLinearVelocity %d", enableLimitLinearVel_);
    zucmotConfig->enableLimitLinearVel = enableLimitLinearVel_;
    zucmot_config_change();
    return 0;
}

int CmdSetEnableLimitJointVelocity::process()
{
    MOTCMD_INFO("CmdSetEnableLimitJointVelocity %d", enableLimitJointVel_);
    zucmotConfig->enableLimitJointVel = enableLimitJointVel_;
    //    tpmanager_SetSafe_enableLimitJointVel(zucmotConfig->enableLimitJointVel);
    zucmot_config_change();
    return 0;
}

int CmdSetPlannerJerk::process()
{
    MOTCMD_INFO("ZUCMOT_SET_PBBLENDMODE %lf, %lf", joint_jerk_, carte_jerk_);
    zucmotConfig->joint_jerk = joint_jerk_;
    zucmotConfig->carte_jerk = carte_jerk_;
    zucmot_config_change();
    return 0;
}

int CmdSetTcpMaxError::process()
{
    MOTCMD_INFO("ZUCMOT_SET_TCP_MAX_ERROR %lf", tcpmaxerror_);
    zucmotConfig->tcp_max_error = tcpmaxerror_;
    zucmot_config_change();
    return 0;
}

int CmdSetPbblendmode::process()
{
    MOTCMD_INFO("ZUCMOT_SET_PBBLENDMODE %d", pb_blend_mode_);
    zucmotConfig->blend_mode = pb_blend_mode_;
    zucmot_config_change();
    return 0;
}

int CmdSetEnableMovejmovelblend::process()
{
    zucmotConfig->enableMovejMovelBlend = enableMovejMovelBlend_;
    //    tpManager_EnableMoveJMoveLBlend(zucmotConfig->enableMovejMovelBlend);
    zucmot_config_change();
    return 0;
}

int CmdSetRateBlend::process()
{
    MOTCMD_INFO("ZUCMOT_SET_RATE_BLEND %d, %d, %d", rate_blend_, topp_queue_len_, max_fw_len_);
    zucmotConfig->rate_blend = rate_blend_;
    zucmotConfig->max_planning_queue_size = topp_queue_len_;
    zucmotConfig->max_fw_len = max_fw_len_;
    zucmot_config_change();
    return 0;
}

int CmdSetEnableMovejmovejblend::process()
{
    zucmotConfig->enableMovejMovejBlend = enableMovejMovejBlend_;
    //    tpManager_enableMovejMovejBlend(zucmotConfig->enableMovejMovejBlend);
    zucmot_config_change();
    return 0;
}

int CmdSetCartJogAcc::process()
{
    for (int i = 0; i < 7; i++)
    {
        zucmotConfig->step_jog_pos_acc[i] = step_jog_pos_acc_[i];
        zucmotConfig->step_jog_ori_acc[i] = step_jog_ori_acc_[i];
        MOTCMD_INFO(" ### zucmotConfig->step_jog_pos_acc[%d] = %f", i, zucmotConfig->step_jog_pos_acc[i]);
        MOTCMD_INFO(" ### zucmotConfig->step_jog_ori_acc[%d] = %f", i, zucmotConfig->step_jog_ori_acc[i]);
    }
    zucmot_config_change();
    return 0;
}

int CmdSetTermCond::process()
{
    MOTCMD_DEBUG("SET_TERM_COND, %d, %lf", termCond_, tolerance_);
    //    tpManager_tpSetTermCond(termCond_, tolerance_);
    return 0;
}

int CmdSetDebug::process()
{
    MOTCMD_DEBUG("SET_DEBUG, %d", debug_);
    zucmotConfig->debug = debug_;
    zucmot_config_change();
    return 0;
}

int CmdFsEnable::process()
{
    /* enable/disable overriding speed */
    /* can happen at any time */
    if (mode_ != 0)
    {
        MOTCMD_DEBUG("FEED SCALE: ON");
        zucmotStatus->enables_new |= FS_ENABLED;
    }
    else
    {
        MOTCMD_DEBUG("FEED SCALE: OFF");
        zucmotStatus->enables_new &= ~FS_ENABLED;
    }
    return 0;
}

int CmdSsEnable::process()
{
    /* enable/disable overriding spindle speed */
    /* can happen at any time */
    if (mode_ != 0)
    {
        MOTCMD_DEBUG("SPINDLE SCALE: ON");
        zucmotStatus->enables_new |= SS_ENABLED;
    }
    else
    {
        MOTCMD_DEBUG("SPINDLE SCALE: OFF");
        zucmotStatus->enables_new &= ~SS_ENABLED;
    }
    return 0;
}

int CmdAfEnable::process()
{
    /* enable/disable adaptive feedrate override from HAL pin */
    /* can happen at any time */
    if (enable_ != 0)
    {
        MOTCMD_DEBUG("ADAPTIVE FEED: ON");
        zucmotStatus->enables_new |= AF_ENABLED;
    }
    else
    {
        MOTCMD_DEBUG("ADAPTIVE FEED: OFF");
        zucmotStatus->enables_new &= ~AF_ENABLED;
    }
    return 0;
}

int CmdSetOperationmodeOption::process()
{
    MOTCMD_INFO("CmdSetOperationmodeOption %d\n", enable_);
    zucmotStatus->operation_mode_option = enable_;
    return 0;
}

int CmdSetDragEnable::active_callback()
{
    if (safety::SafeIOMgr::instance().is_protective_stop())
    {
        // rcs_print("Cannot do JOG when in protective_stop status\n");
        return -1;
    }
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
    if (status.is_sim)
    {
        MOTCMD_ERR("Robot %d is in simulation mode, cannot do DRAG", group_id_);
        return -1;
    }
    return 0;
}

int CmdSetDragEnable::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (robot->get_cur_plan_mode() != axisgroup::PFsmState::PlanMode::PLAN_DRAG)
    {
        if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
        {
            MOTCMD_ERR("Robot %d is not ready for a drag command : moving state: %s, plan mode : %s",
                       group_id_,
                       axisgroup::AxisGroup::run_state_string(robot->get_state()),
                       axisgroup::Robot::get_plan_mode_stirng(robot->get_cur_plan_mode()));
            return -1;
        }
    }
    if (enable_)
    {
        robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_DRAG);  // 进入拖拽
    }
    else if (robot->get_cur_plan_mode() == axisgroup::PFsmState::PlanMode::PLAN_DRAG)
    {
        auto drag = robot->get_drag_pfsm();
        drag->exit_mode();
    }
    return 0;
}

int CmdSetRobotPayload::active_callback()
{
    if (id_ >= (int)axisgroup::RobotPayloadMgr::MAX_ROBOT_PAYLOAD_NUM)
    {
        return -1;
    }
    return 0;
}

int CmdSetRobotPayload::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->get_set_intf()->set_payload(id_);
    return 0;
}

int CmdSetPayload::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->get_set_intf()->set_payload(id_);
    return 0;
}

int CmdServoHome::active_callback()
{
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();

    if (joint_ > status.robot_joint_num || joint_ < 0)
    {
        MOTCMD_ERR("Servo home joint %d is not valid", group_id_, joint_);
        return -1;
    }
    if (status.is_sim)
    {
        MOTCMD_ERR("Robot %d is in simulation mode, cannot do SERVO HOME", group_id_);
        return -1;
    }
    return 0;
}

int CmdServoHome::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (robot->is_homing())
    {
        MOTCMD_ERR("Robot %d is already homing", group_id_);
        return -1;
    }
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
    {
        MOTCMD_ERR("Robot %d is not ready for a servo home command : moving state: %s, plan mode : %s",
                   group_id_,
                   axisgroup::AxisGroup::run_state_string(robot->get_state()),
                   axisgroup::Robot::get_plan_mode_stirng(robot->get_cur_plan_mode()));
        return -1;
    }
    else
    {
        // 切换到IDLE规划模式
        if (robot->get_cur_plan_mode() != axisgroup::PFsmState::PlanMode::PLAN_IDLE)
        {
            robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_IDLE);
        }
    }
    robot->do_servo_home(joint_);
    return 0;
}

int CmdClearError::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (robot->get_state() != axisgroup::AxisGroup::FsmState::ErrorStop)
    {
        return -1;
    }
    if (robot->is_enabled())
    {
        robot->change_fsm_stat(axisgroup::AxisGroup::FsmState::Standby);
    }
    else
    {
        robot->change_fsm_stat(axisgroup::AxisGroup::FsmState::Disabled);
    }
    return 0;
}

int CmdServojop::active_callback()
{
    if (safety::SafeIOMgr::instance().is_protective_stop())
    {
        // rcs_print("Cannot do JOG when in protective_stop status\n");
        return -1;
    }
    return 0;
}

int CmdServojop::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    if (enable_)
    {
        if (robot->get_state() != axisgroup::AxisGroup::FsmState::Standby)
        {
            MOTCMD_ERR("Robot %d is not ready for a servo command : moving state: %s, plan mode : %s",
                       group_id_,
                       axisgroup::AxisGroup::run_state_string(robot->get_state()),
                       axisgroup::Robot::get_plan_mode_stirng(robot->get_cur_plan_mode()));
            return -1;
        }
        // 自动切至free模式
        robot->switch_plan_mode(axisgroup::PFsmState::PlanMode::PLAN_SERVO);
    }
    else
    {
        if (robot->get_cur_plan_mode() == axisgroup::PFsmState::PlanMode::PLAN_SERVO)
        {
            robot->motion_abort();
        }
    }
    return 0;
}

int DynCmdStartDynamicsIdentify::active_callback()
{
    int err = CmdServojop::active_callback();
    if (err)
    {
        return err;
    }
    return 0;
}

int DynCmdStartDynamicsIdentify::process()
{
    CmdServojop::process();
    MOTCMD_INFO("ZUCMOT_START_DYNAMICS_IDENTIFY\n");
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    auto servo = std::static_pointer_cast<axisgroup::PFsmServo>(robot->get_servo_pfsm());
    if (servo->dynamics_identifying_status_ = axisgroup::PFsmServo::DynIdentifyStatus::DYN_STAND_BY)
    {
        return -1;
    }
    servo->dynamics_identifying_status_ = axisgroup::PFsmServo::DynIdentifyStatus::DYN_IN_MOV;
    return 0;
}

int CmdSetBaseOffset::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->get_set_intf()->set_base_offset();
    return 0;
}

int CmdSetSimRobot::active_callback()
{
    auto status = mot::al::MotionProxy::instance().robot(group_id_)->get_status();
    if (!status.is_sim)
    {
        MOTCMD_ERR("Robot %d is not in simulation mode", group_id_);
        return -1;
    }
    if (status.is_power_on || status.is_powering)
    {
        MOTCMD_ERR("Robot %d is powered on, cannot set to simulation mode", group_id_);
        return -1;
    }
    if (status.is_enable || status.is_enabling)
    {
        MOTCMD_ERR("Robot %d is enabled, cannot set to simulation mode", group_id_);
        return -1;
    }
    return 0;
}

int CmdSetSimRobot::process()
{
    auto robot = axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_);
    robot->set_sim(true, sim_serial_num_);
    return 0;
}
